import sensor, image, time,math,pyb
from pyb import UART,LED
import json
import ustruct

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
red_threshold_01=(10, 100, 127, 32, -43, 67)
clock = time.clock()

uart = UART(3,115200)   #定义串口3变量
uart.init(115200, bits=8, parity=None, stop=1) # init with given parameters

fx=0;fy=0;

def find_max(blobs):    #定义寻找色块面积最大的函数
    max_size=0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob=blob
            max_size = blob.pixels()
    return max_blob


while(True):
    clock.tick()
    img = sensor.snapshot()
    #blobs = img.find_blobs([red_threshold_01],roi=[0,0,320,20])
    #blobs2 = img.find_blobs([red_threshold_01],roi=[0,0,320,20])
    blobs = img.find_blobs([red_threshold_01])
    cx=0;cy=0;
    if blobs:
            max_b = find_max(blobs)
            #如果找到了目标颜色
            cx=max_b[5]
            cy=max_b[6]
            img.draw_rectangle(max_b[0:4]) # rect
            img.draw_cross(max_b[5], max_b[6]) # cx, cy
            fx=max_b[5]
            fy=max_b[6]
            print(max_b[2]*max_b[3])
            output_str="%d %d " % (fx,fy)
            uart.write(output_str+'\n')
img = img.to_grayscale()
